﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Windows.Forms.DataVisualization.Charting;

namespace UltrasonicTDPlatform
{
    public partial class SweepForm : Form
    {
        /*
         * 用于存放显示数据的队列，以及队列的一些信息
         */
        public static int pointLength = 512;



        public int curValue = 0;
        public int num = 5;//每次删除增加几个点

        // 扫频起始和终止频率
        public static float START_FREQUECY = 20000;
        public static float END_FREQUECY = 20300;

        public static double SWEEP_STEP = 0.5;

        //扫频电压大小
        public static float SweepVoltage = 1;

        // 负载大小
        public static float LoadSize = 0;

        // 启动自动扫频标志
        bool autoSweep = false;

        public SweepForm()
        {
            InitializeComponent();
            this.TopLevel = false;

            //InitChart(this.chart1, -12, 12, "驱动电压实时波形显示", "幅值 / V");
            //InitChart(this.chart2, -0.3, 0.3, "驱动电流实时波形显示", "幅值 / A");

            chart_up_left.ChartAreas[0].AxisX.LabelStyle.Format = "F0";//2位小数
            chart_up_left.ChartAreas[0].AxisY.LabelStyle.Format = "F2";//2位小数

            chart_up_right.ChartAreas[0].AxisX.LabelStyle.Format = "F0";//2位小数
            chart_up_right.ChartAreas[0].AxisY.LabelStyle.Format = "F2";//2位小数

            chart_down.ChartAreas[0].AxisX.LabelStyle.Format = "F2";//2位小数
            chart_down.ChartAreas[0].AxisY.LabelStyle.Format = "F2";//2位小数

            chart_up_left.ChartAreas[0].AxisX.Minimum = START_FREQUECY;
            chart_up_left.ChartAreas[0].AxisX.Maximum = END_FREQUECY;

            chart_up_right.ChartAreas[0].AxisX.Minimum = START_FREQUECY;
            chart_up_right.ChartAreas[0].AxisX.Maximum = END_FREQUECY;

            chart_up_left.ChartAreas[0].AxisX.Interval = (END_FREQUECY - START_FREQUECY)/5;
            chart_up_right.ChartAreas[0].AxisX.Interval = (END_FREQUECY - START_FREQUECY) / 5;

            // 初始化扫频起始和终止
            sweepStartFreNumericBox.Value = (decimal)START_FREQUECY;
            sweepEndFreNumericBox.Value = (decimal)END_FREQUECY;


            //chart_up.ChartAreas[0].AxisY.Minimum = 0;
            //chart_up.ChartAreas[0].AxisY.Maximum = 10;



            //chart_up.ChartAreas[0].AxisY.IsStartedFromZero = false;
            //chart_up.ChartAreas[0].AxisX.IsStartedFromZero = false;
            //chart_up.ChartAreas[0].AxisY2.IsStartedFromZero = false;
            //chart_up.ChartAreas[0].AxisX2.IsStartedFromZero = false;

        }



        private void Sweep_Load(object sender, EventArgs e)
        {
            
        }

        /*
        启动按钮关联函数
        作用：将扫频范围、扫频模式、扫频电压等传递给下位机。
        输入： 无
        输出： 空
        */
        //启动按钮函数
        private void button1_Click(object sender, EventArgs e)
        {
            if (button1.Text.Equals("开始自动扫频"))
            {
                numericUpDown1.Value = (decimal)START_FREQUECY;
                sendCommand();
                button1.Text = "停止自动扫频";
                numericUpDown1.Enabled = false;
                numericUpDown2.Enabled = false;
                button1.Enabled = false;

                autoSweep = true;
            }
            else
            {
                button1.Text = "开始自动扫频";
                numericUpDown1.Enabled = true;
                numericUpDown2.Enabled = true;
                button1.Enabled = true;
      
                autoSweep = false;
            }
        }


        /*
        BVD模型构建函数
        作用：通过辨识得到的BVD参数，计算阻抗特性、相位特性。
        输入： C0：静态电容 R1：动态电阻  C1：动态电容  L1：动态电感。
        输出： 空
        */
        //根据BVD模型计算阻抗和相位
        private void construct_model(double C0,double R1,double C1,double L1) {

/*            long[] fre_store = new long[end_fre - start_fre+1];
            double[] Phase_store = new double[end_fre - start_fre+1];
            double[] IMP_store = new double[end_fre - start_fre+1];
            double[] G_img = new double[end_fre - start_fre+1];
            double[] G_real = new double[end_fre - start_fre+1];

            double G_img_max = 0;
            double G_img_min = 0;

            double G_real_max =-10000;
            double G_real_min = 10000;

            double IMP_min = -1;

            long resonate_fre = start_fre;

            C0 = C0 * Math.Pow(10,-9);
            C1= C1 *Math.Pow(10, -9);
            L1 = L1 * Math.Pow(10, -3);
            double W = 0;

            double a1 = 0.0;
            double b1 = 0.0;
            double a2 = 0.0;
            double b2 = 0.0;
            double real = 0.0;
            double img = 0.0;


            

            for (long i = start_fre; i <= end_fre; i++) {
                W = 2 * Math.PI * i;
                a1 = L1 * C1 * W * W - 1;
                b1 = -R1 * C1 * W;
                a2 = R1 * C1 * C0 * W * W;
                b2 = -(C0 + C1) * W + L1 * C1 * C0 * W * W * W;

                //这是阻抗。
                real = (a1 * a2 + b1 * b2) / (a2 * a2 + b2 * b2);
                img = (a2 * b1 - a1 * b2) / ((a2 * a2 + b2 * b2));

                long count = i - start_fre;

                fre_store[count] = i;
                IMP_store[count] = Math.Sqrt(real * real + img * img);
                Phase_store[count] = Math.Atan(img / real) * (180 / Math.PI);
                G_img[count] = -(img / (img * img + real * real));
                G_real[count] = real / (img * img + real * real);

                if (IMP_min >= IMP_store[count]) {
                    IMP_min = IMP_store[count];
                    resonate_fre = i;
                }
                G_real_max = Math.Max(G_real_max, G_real[count]);
                G_real_min = Math.Min(G_real_min, G_real[count]);

                G_img_max = Math.Max(G_img_max, G_img[count]);
                G_img_min = Math.Min(G_img_min, G_img[count]);

                //设置成一个主轴，一个副轴就好。
                chart_up.Series["IMP"].Points.AddXY(i, Math.Log10(IMP_store[count]));

                chart_up.Series["Phase"].Points.AddXY(i, Phase_store[count]);

                chart_down.Series["circle"].Points.AddXY( G_real[count], G_img[count]);

            }
            //扫频亮灯
            pictureBox_Sweep.Image = UltrasonicTDPlatform.Properties.Resources.led_light;


            chart_up.ChartAreas[0].AxisX.Interval = (end_fre- start_fre)/5;

            chart_down.ChartAreas[0].AxisX.Maximum = 1.5 * G_real_max - 0.5 * G_real_min;
            chart_down.ChartAreas[0].AxisX.Minimum = 1.5 * G_real_min - 0.5 * G_real_max;

            chart_down.ChartAreas[0].AxisX.Interval = G_real_max - G_real_min;*/

        }

        /*
        Taubin拟合法函数
        作用：拟合导纳圆，获取半径以及圆心。
        输入： x：导纳圆实部数据   y：导纳圆虚部数据 。
        输出： 空
        */
        //
        private void Tuabin_fit(double[] x,double[] y ,long deltaF ,long resonateFre) {

            int n = x.Length;
            double res_x = 0;
            double res_y = 0;
            for (int i = 0; i < n; i++) {
                res_x += x[i];
                res_y += y[i];
            }

            double mean_x = res_x / n;
            double mean_y = res_y / n;
            double Mxx = 0, Myy = 0, Mxy = 0, Mxz = 0, Myz = 0, Mzz = 0;
            double Xi = 0, Yi = 0, Zi = 0;

            for (int i = 0; i < n; i++) {
                Xi = x[i] - mean_x;
                Yi = y[i] - mean_y;
                Zi = Xi * Xi + Yi * Yi;
                Mxy = Mxy + Xi * Yi;
                Mxx = Mxx + Xi * Xi;
                Myy = Myy + Yi * Yi;
                Mxz = Mxz + Xi * Zi;
                Myz = Myz + Yi * Zi;
                Mzz = Mzz + Zi * Zi;
            }
            Mxx = Mxx / n;
            Myy = Myy / n;
            Mxy = Mxy / n;
            Mxz = Mxz / n;
            Myz = Myz / n;
            Mzz = Mzz / n;

            double Mz = Mxx + Myy;
            double Cov_xy = Mxx * Myy - Mxy * Mxy;
            double A3 = 4 * Mz;
            double A2 = -3 * Mz * Mz - Mzz;
            double A1 = Mzz * Mz + 4 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz - Mz * Mz * Mz;
            double A0 = Mxz * Mxz * Myy + Myz * Myz * Mxx - Mzz * Cov_xy - 2 * Mxz * Myz * Mxy + Mz * Mz * Cov_xy;
            double A22 = A2 + A2;
            double A33 = A3 + A3 + A3;

            double xnew = 0.0;
            double ynew = 1e+20;
            double epsilon = 1e-12;
            int IterMax = 20;

            double yold = ynew;
            double xold = xnew;
            for (int iter_counts = 0; iter_counts < IterMax; iter_counts++) {
                yold = ynew;

                ynew = A0 + xnew * (A1 + xnew * (A2 + xnew * A3));
                if (Math.Abs(ynew) > Math.Abs(yold)) {
                    xnew = 0;
                    break;
                }

                double Dy = A1 + xnew * (A22 + xnew * A33);
                xold = xnew;
                xnew = xold - ynew / Dy;

                if (Math.Abs((xnew - xold) / xnew) < epsilon) {
                    break;
                }

                if (iter_counts >= IterMax) {
                    break;
                    xnew = 0;
                }
                if (xnew < 0) {
                    xnew = 0;
                }

            }

            double DET = xnew * xnew - xnew * Mz + Cov_xy;

            double Center_x = Mxz * (Myy - xnew) - Myz * Mxy / DET / 2;
            double Center_y = Myz * (Mxx - xnew) - Mxz * Mxy / DET / 2;

            double x_cal = Center_x + mean_x;
            double y_cal = Center_y + mean_y;
            double r_cal = Math.Sqrt(Center_x * Center_x + Center_y * Center_y + Mz);

        }

        private void button2_Click(object sender, EventArgs e)
        {
            if (MainMenu.serialPort1 == null || !MainMenu.serialPort1.IsOpen)
                return;

            if (button2.Text.Equals("开始扫频"))
            {
                numericUpDown1.Value = (decimal)START_FREQUECY;
                button2.Text = "停止扫频";
                numericUpDown1.Enabled = false;
                numericUpDown2.Enabled = false;
                button1.Enabled = false;
                autoSweep = true;

                chart_up_left.Series["Phase"].Points.Clear();
                chart_up_right.Series["IMP"].Points.Clear();
                chart_down.Series["circle"].Points.Clear();


                timer1.Start();

            }
            else
            {
                button2.Text = "开始扫频";
                numericUpDown1.Enabled = true;
                numericUpDown2.Enabled = true;
                button1.Enabled = true;
                autoSweep = false;

                timer1.Stop();
            }
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            timer1.Stop();
            // 判断是否自动扫频
            if (autoSweep)
            {
                // 扫频到头了
                if (float.Parse(numericUpDown1.Value.ToString()) > END_FREQUECY)
                {
                    autoSweep = false;
                    button2.Text = "开始扫频";
                    numericUpDown1.Enabled = true;
                    numericUpDown2.Enabled = true;
                    button1.Enabled = true;
                    autoSweep = false;
                    timer1.Stop();
                    saveFile();
                    MessageBox.Show("扫频完成！\n 扫频范围：" + START_FREQUECY + "Hz - " + END_FREQUECY + "Hz", "Success");
                    
                    return;
                }
            }

            // 1. 先向DSP写命令，ff00表示下位机向上位机输送一次数据
            MainMenu.serialPort1.Write(new Byte[] { 0xff, 0x80 }, 0, 2);

            // 2. 再接收DSP的数据
            while (MainMenu.serialPort1.ReadByte() != 0x22) ;
            byte[] buffer = new byte[4];

            // 2.1. 接收频率
            MainMenu.serialPort1.Read(buffer, 0, 4);
            double f = 0;
            f += (double)buffer[0];
            f = f * 256;
            f += (double)buffer[1];
            f = f * 256;
            f += (double)buffer[2];
            f = f * 256;
            f += (double)buffer[3];
            
            f = f / 100;

            // 2.2. 接收相位
            MainMenu.serialPort1.Read(buffer, 0, 4);
            double phase = 0;
            phase += (double)buffer[0];
            phase = phase * 256;
            phase += (double)buffer[1];
            phase = phase * 256;
            phase += (double)buffer[2];
            phase = phase * 256;
            phase += (double)buffer[3];
            phase = phase - 1000;
            phase = phase * (f / 1000) * 0.018;
            

            // 3.3. 接收电压
            MainMenu.serialPort1.Read(buffer, 0, 4);
            double v = 0;
            v += (double)buffer[0];
            v = v * 256;
            v += (double)buffer[1];
            v = v * 256;
            v += (double)buffer[2];
            v = v * 256;
            v += (double)buffer[3];

            // 3.4. 接收电流
            MainMenu.serialPort1.Read(buffer, 0, 4);
            double i = 0;
            i += (double)buffer[0];
            i = i * 256;
            i += (double)buffer[1];
            i = i * 256;
            i += (double)buffer[2];
            i = i * 256;
            i += (double)buffer[3];

            // 4. 计算阻抗
            double impedance = v / i;

            
            // 5.1 阻抗、相位曲线
            
            chart_up_left.Series["Phase"].Points.AddXY(f, phase);
            chart_up_right.Series["IMP"].Points.AddXY(f, impedance*50);

            textBox_phase.Text = phase.ToString("F2");
            textBox_frequency.Text = f.ToString("F2");
            textBox_imp.Text = impedance.ToString("F2");

            // 5.2 显示导纳圆
            double G_real = 1 / impedance * Math.Cos(-phase * Math.PI/180);
            double G_img = 1 / impedance * Math.Sin(-phase * Math.PI / 180);
            chart_down.Series["circle"].Points.AddXY(G_real, G_img);

            // 6. 最后再次发送驱动命令，更新驱动频率为下一次的频率，为下一次读取做准备
            numericUpDown1.Value = numericUpDown1.Value + (decimal)(SWEEP_STEP);
            sendCommand();
            timer1.Start();
        }


        /**
         * 将阻抗和相位数组写入文件
         */
        private void saveFile()
        {
            DataPoint[] impData = chart_up_right.Series["IMP"].Points.ToArray();
            DataPoint[] phaseData = chart_up_left.Series["Phase"].Points.ToArray();

            string fileName = "/FreSweep/DriveVol-"
                    + SweepVoltage + "V-LoadSize-"
                    + LoadSize + "kg" + ".txt";

            //第一个参数是读取到流的文件名
            //第二个参数是如果文件不存在，能否创建文件，true为创建新文件，false为不创建
            StreamWriter sw = new StreamWriter(fileName, false);
            for(int i = 0; i < impData.Length; i++)
            {
                sw.Write(impData[i].XValue);
                sw.Write(" ");
                sw.Write(impData[i].YValues[0]);
                sw.Write(" ");
                sw.Write(phaseData[i].YValues[0]);
                sw.WriteLine();
            }
            sw.Flush();
            sw.Close();
            sw.Dispose();

        }

        private void numericUpDown2_ValueChanged(object sender, EventArgs e)
        {
            sendCommand();
        }

        private void numericUpDown1_ValueChanged(object sender, EventArgs e)
        {
            sendCommand();
        }

        private void sendCommand()
        {
            if ( !MainMenu.serialPort1.IsOpen)
                return;
            // 频率大小是数字框里的数字，乘以100再发送
            int value = (int)(numericUpDown1.Value * 100);//
            byte[] CMD = new byte[6];
            // 1. 帧头
            CMD[0] = 0xff;
            CMD[1] = 0xfe;
            // 2. 驱动频率
            CMD[2] = (byte)((value >> 24) & 0xFF);
            CMD[3] = (byte)((value >> 16) & 0xFF);
            CMD[4] = (byte)((value >> 8) & 0xFF);
            CMD[5] = (byte)(value & 0xFF);//低位
            // 3. 驱动占空比
            //CMD[6] = (byte)(0xff * numericUpDown2.Value / 100);
            // 3. 发送
            MainMenu.serialPort1.Write(CMD, 0, CMD.Length);
        }

       

        private void button1_Click_1(object sender, EventArgs e)
        {
            saveFile();
        }

        private void chart2_Click(object sender, EventArgs e)
        {

        }

        private void chart1_Click(object sender, EventArgs e)
        {

        }

        private void groupBox4_Enter(object sender, EventArgs e)
        {

        }

        private void panel2_Paint(object sender, PaintEventArgs e)
        {

        }

        private void panel1_Paint(object sender, PaintEventArgs e)
        {

        }

        private void sweepStartFreNumericBox_ValueChanged(object sender, EventArgs e)
        {
            START_FREQUECY = (float)sweepStartFreNumericBox.Value;
            chart_up_left.ChartAreas[0].AxisX.Minimum = START_FREQUECY;
        }

        private void sweepEndFreNumericBox_ValueChanged(object sender, EventArgs e)
        {
            END_FREQUECY = (float)sweepEndFreNumericBox.Value;
            chart_up_left.ChartAreas[0].AxisX.Maximum = END_FREQUECY;
        }
    }
}
